﻿#include "opengl_demo/render_form.h"
#include "./ui_render_form.h"

#include <QGridLayout>
#include <QFile>
#include <QtMath>
#include <QMatrix4x4>

#include <qrender/render_manager.h>
#include <qrender/camera.h>
#include <qrender/q3ds_renderer.h>
#include <qrender/line_renderer.h>
#include <qrender/axis_renderer.h>
#include <qrender/sphere_renderer.h>
#include <qrender/point_cloud_renderer.h>
#include <qrender/box_renderer.h>

#include <settings/qls_settings.h>

#include "math.h"

// 相机坐标
#define CAMERA_INIT_Y 550
#define CAMERA_INIT_Z 2500

// 固定坐标系到相机的距离
#define CAMERA_INIT_DISTANCE_LENGTH 430
// 固定坐标系尺寸
#define AXIS_DEFAULT_CONE_HEIGHT (20.0 * (0.8 / 2.2) * 0.5)
#define AXIS_DEFAULT_CONE_RADIUS (5.0 * (0.8 / 2.2) * 0.5)

// 基座位置
const std::vector<double> base_pose_ = { 0.0, 0.0, -0.025, 0.0, 0.0, 0.0 };

// 目标机械臂滤镜色值
const QVector4D target_robot_filter_color_ = { 0.706, 0.0, 0.0, 0.5 };

RenderForm::RenderForm(QWidget *parent)
    : RenderView(parent), ui(new Ui::RenderForm)
{
    qDebug() << __FUNCTION__;

    ui->setupUi(this);

    // 底座
    r_base_.reset(new BoxRenderer());
    auto world_pose = Math::robotToWorld(base_pose_);
    r_base_->setPose(world_pose);

    r_base_->setShape(1000, 1000, 50);
    r_base_->setTexture("img:base.bmp");
    render_manager_->addRenderer(r_base_);

    // 真实机械臂
    r_real_robot_.reset(new Q3dsRenderer());
    render_manager_->addRenderer(r_real_robot_);

    // 末端
    r_real_robot_pose_.reset(new AxisRenderer());
    render_manager_->addRenderer(r_real_robot_pose_);

    // 目标机械臂
    r_target_robot_.reset(new Q3dsRenderer());
    r_target_robot_->setFilter(target_robot_filter_color_);
    render_manager_->addRenderer(r_target_robot_);

    // 世界坐标系
    r_world_pose_.reset(new AxisRenderer());
    r_world_pose_->setShape(AXIS_DEFAULT_CONE_HEIGHT, AXIS_DEFAULT_CONE_RADIUS);
    QMatrix4x4 fixed_matrix;
    fixed_matrix.lookAt({ 0, CAMERA_INIT_Y, CAMERA_INIT_Z },
                        { 0, CAMERA_INIT_Y, 0 }, { 0, 1, 0 });
    r_world_pose_->setFixed(true, fixed_matrix);
    render_manager_->addRenderer(r_world_pose_);

    // 相机
    camera_ = render_manager_->getCamera();
    camera_->setRotateEnable({ false, true, false });
    camera_->setMouseScaleFactor(60.0);
    initView();

    setupUi();
}

RenderForm::~RenderForm()
{
    qDebug() << __FUNCTION__;
}

bool RenderForm::reload(
    const QString &robot_type,
    const std::unordered_map<std::string, std::vector<double>> &dh)
{
    auto dirs = QStringList()
                << QLsSettings::getInstallPath() << QLsSettings::getWorkSpace();
    return r_real_robot_->reload(dirs, robot_type, dh) &&
           r_target_robot_->reload(dirs, robot_type, dh);
}

double RenderForm::getViewRotationAngleX()
{
    return qDegreesToRadians(camera_->getRotateAngle().y());
}

double RenderForm::getViewRotationAngleY()
{
    return qDegreesToRadians(camera_->getRotateAngle().x());
}

double RenderForm::getViewRotationAngleZ()
{
    return qDegreesToRadians(camera_->getRotateAngle().z());
}

void RenderForm::setViewRotationAngleX(double angle)
{
    camera_->rotateX(angle);
}

void RenderForm::setViewRotationAngleY(double angle)
{
    camera_->rotateY(angle);
}

void RenderForm::setViewRotationAngleZ(double angle)
{
    camera_->rotateZ(angle);
}

void RenderForm::setRobotGravity(std::vector<double> gravity_component)
{
    if (gravity_component.size() != 3 ||
        gravity_component_ == gravity_component) {
        return;
    }
    gravity_component_ = gravity_component;

    qDebug() << __FUNCTION__ << gravity_component[0] << gravity_component[1]
             << gravity_component[2];

    // NOTE: 此为机器人竖直向上安装的分量值
    // TODO: http://git.aubo-robotics.cn:8001/arcs/TR/-/issues/1535#note_118264
    gravity_component[0] = 0;
    gravity_component[1] = 0;
    gravity_component[2] = -9.81;

    camera_->setGravityComponent(QVector3D(
        gravity_component[0], gravity_component[1], gravity_component[2]));
    camera_->translate(0, default_down_move_, default_zoom_out_);
}

void RenderForm::setRenderEmbeded(bool isEmbeded, QGridLayout *gly)
{
    auto item = gly->itemAtPosition(0, 0);
    if (isEmbeded) {
        if (item && item->widget() == this) {
            return;
        }

        gly->addWidget(this, 0, 0, 1, 1);
        show();
        update();
    } else {
        if (!item || item->widget() != this) {
            return;
        }

        hide();
        gly->removeWidget(this);

        // addWidget会设上parent，removeWidget却不置空parent！
        this->setParent(nullptr);
    }
}

void RenderForm::setHasTarget(bool visiable)
{
    has_target_ = visiable;

    updateTargetVisiable();
}

void RenderForm::setTargetVisiable(bool visiable)
{
    target_visiable_ = visiable;

    updateTargetVisiable();
}

void RenderForm::setMoveForm(QWidget *main_view)
{
    main_view_ = main_view;
}

void RenderForm::setRotateEnable(bool rotate_enable)
{
    camera_->setRotateEnableY(false);
}

void RenderForm::setDefaultSizeScale(int zoom_out_value, int down_move_value)
{
    default_zoom_out_ = -5 * zoom_out_value;
    default_down_move_ = -3 * down_move_value;

    initView();
}

void RenderForm::resizeGL(int w, int h)
{
    // 这是一个比较适合的位置和尺寸
    r_world_pose_->setPose(Math::robotToWorld(
        { 2.2, -0.11 * w / h, 2.2 * (0.55 / 2.5) * 0.91, 0.0, 0.0, 0.0 }));

    return RenderView::resizeGL(w, h);
}

void RenderForm::qssInit()
{
#ifdef QSS_ENABLE
    QFile file(":/style.qss");
    if (!file.open(QIODevice::ReadOnly)) {
        return;
    }

    QTextStream stream(&file);
    QString qss = stream.readAll();
    setStyleSheet("");
    setStyleSheet(qss);
#endif
}

void RenderForm::setupUi()
{
    qssInit();

    // 基座显隐
    connect(ui->pbn_ground, &QPushButton::clicked, this, [=](bool checked) {
        r_base_->setVisible(!ui->pbn_ground->isHidden() && checked);
        //        Logger::normalOperationLog(
        //            tr("%1 ground.").arg(checked ? tr("Show") : tr("Hide")));
    });

    // 当前机器人显隐
    connect(ui->pbn_real_robot, &QPushButton::clicked, this, [=](bool checked) {
        auto visiable = !ui->pbn_real_robot->isHidden() && checked;
        r_real_robot_->setVisible(visiable);
        r_real_robot_pose_->setVisible(visiable);

        //        Logger::normalOperationLog(
        //            tr("%1 real robot.").arg(checked ? tr("Show") :
        //            tr("Hide")));
    });

    // 目标机器人显隐
    connect(ui->pbn_target_robot, &QPushButton::clicked, this,
            [=](bool checked) {
                updateTargetVisiable();

                //            Logger::normalOperationLog(
                //                tr("%1 target robot.").arg(checked ?
                //                tr("Show") : tr("Hide")));
            });

    // 用户坐标系 坐标轴显隐
    connect(ui->pbn_coord, &QPushButton::clicked, this, [=](bool checked) {
        auto visiable =
            coordinates_visiabled_ || !ui->pbn_coord->isHidden() && checked;
        for (auto &c : r_coordinates_axis_) {
            c->setVisible(visiable);
        }
        for (auto &c : r_coordinates_) {
            c->setVisible(visiable);
        }

        //        Logger::normalOperationLog(
        //            tr("%1 coordinate.").arg(checked ? tr("Show") :
        //            tr("Hide")));
    });

    // 路点显隐
    connect(ui->pbn_point, &QPushButton::clicked, this, [=](bool checked) {
        auto visiable = !ui->pbn_point->isHidden() && checked;
        for (auto &waypoint : waypoints_) {
            waypoint.r_waypoints_->setVisible(visiable);
        }

        //        Logger::normalOperationLog(
        //            tr("%1 waypoint.").arg(checked ? tr("Show") :
        //            tr("Hide")));
    });

    // 轨迹显隐
    connect(ui->pbn_track, &QPushButton::clicked, this, [=](bool checked) {
        auto visiable = !ui->pbn_track->isHidden() && checked;
        for (auto &waypoint : waypoints_) {
            waypoint.r_traj_->setVisible(visiable);
        }

        //        Logger::normalOperationLog(
        //            tr("%1 track.").arg(checked ? tr("Show") : tr("Hide")));
    });

    // 缩小
    connect(ui->pbn_zoom_out, &QPushButton::clicked, this,
            [=] { camera_->scale(-5); });
    //    connect(ui->pbn_zoom_out, &QPushButtonPlus::pressed0, this,
    //            [=]() { Logger::normalOperationLog(tr("Zoom out")); });

    // 上移
    connect(ui->pbn_up_move, &QPushButton::clicked, this,
            [=] { camera_->translate(0, 3, 0); });
    //    connect(ui->pbn_up_move, &QPushButtonPlus::pressed0, this,
    //            [=]() { Logger::normalOperationLog(tr("Move up")); });

    // 放大
    connect(ui->pbn_zoom_in, &QPushButton::clicked, this,
            [=] { camera_->scale(5); });
    //    connect(ui->pbn_zoom_in, &QPushButtonPlus::pressed0, this,
    //            [=]() { Logger::normalOperationLog(tr("Zoom in")); });

    // 左移
    connect(ui->pbn_left_move, &QPushButton::clicked, this,
            [=] { camera_->translate(-3, 0, 0); });
    //    connect(ui->pbn_left_move, &QPushButtonPlus::pressed0, this,
    //            [=]() { Logger::normalOperationLog(tr("Move left")); });

    // 还原
    connect(ui->pbn_front_view, &QPushButton::clicked, this,
            [=] { initView(); });
    //    connect(ui->pbn_front_view, &QPushButtonPlus::pressed0, this,
    //            [=]() { Logger::normalOperationLog(tr("Front view")); });

    // 右移
    connect(ui->pbn_right_move, &QPushButton::clicked, this,
            [=] { camera_->translate(3, 0, 0); });
    //    connect(ui->pbn_right_move, &QPushButtonPlus::pressed0, this,
    //            [=]() { Logger::normalOperationLog(tr("Move right")); });

    // 左旋转
    connect(ui->pbn_left_rotate, &QPushButton::clicked, this,
            [=] { camera_->rotateY(-1); });
    //    connect(ui->pbn_left_rotate, &QPushButtonPlus::pressed0, this,
    //            [=]() { Logger::normalOperationLog(tr("Rotate left")); });

    // 下移
    connect(ui->pbn_down_move, &QPushButton::clicked, this,
            [=] { camera_->translate(0, -3, 0); });
    //    connect(ui->pbn_down_move, &QPushButtonPlus::pressed0, this,
    //            [=]() { Logger::normalOperationLog(tr("Move down")); });

    // 右旋转
    connect(ui->pbn_right_rotate, &QPushButton::clicked, this,
            [=] { camera_->rotateY(1); });
    //    connect(ui->pbn_right_rotate, &QPushButtonPlus::pressed0, this,
    //            [=]() { Logger::normalOperationLog(tr("Rotate right")); });

    ui->pbn_real_robot->setVisible(true);

    ui->pbn_ground->setChecked(true);
    emit ui->pbn_ground->clicked(true);

    ui->pbn_real_robot->setChecked(true);
    emit ui->pbn_real_robot->clicked(true);

    ui->pbn_target_robot->setChecked(false);
    emit ui->pbn_target_robot->clicked(false);

    ui->pbn_coord->setChecked(false);
    emit ui->pbn_coord->clicked(false);

    ui->pbn_track->setChecked(true);
    emit ui->pbn_track->clicked(true);

    ui->pbn_point->setChecked(true);
    emit ui->pbn_point->clicked(true);
}

void RenderForm::updateTargetVisiable()
{
    r_target_robot_->setVisible(
        target_visiable_ || (has_target_ && !ui->pbn_target_robot->isHidden() &&
                             ui->pbn_target_robot->isChecked()));
}

void RenderForm::setRealJointPositions(const std::vector<double> &position)
{
    if (position.size() != 6) {
        return;
    }

    r_real_robot_->setJointPositions(position);
}

void RenderForm::setTargetJointPositions(const std::vector<double> &position)
{
    if (position.size() != 6) {
        return;
    }

    r_target_robot_->setJointPositions(position);
}

void RenderForm::setTcpPose(const std::vector<double> &pose)
{
    if (pose.size() != 6) {
        return;
    }

    // x, y, z 单位是 m, 需要转换为 mm 再计算
    r_real_robot_pose_->setPose(Math::robotToWorld(pose));
}

QPixmap *RenderForm::grabFromJoints(const std::vector<double> &joints,
                                    const std::vector<double> &pose)
{
    if (joints.size() != 6 || pose.size() != 6) {
        return nullptr;
    }

    static double last_roty = 0;

    double roty = camera_->getRotateAngle().y();

    // 获取关节和位姿的哈希值
    std::vector<double> vec_data;
    vec_data.insert(vec_data.end(), joints.begin(), joints.end());
    vec_data.insert(vec_data.end(), pose.begin(), pose.end());
    QByteArray tmp_data;
    tmp_data.setRawData(reinterpret_cast<const char *>(vec_data.data()),
                        vec_data.size() * sizeof(double));
    quint64 key_val = qHash(tmp_data);

    // 缓存机制 相机发生变化需要更新缓存
    if ((last_roty == roty) && cache_pixmap_.contains(key_val)) {
        return cache_pixmap_.object(key_val);
    }

    // 不在显示窗口内无法 grab 到界面
    auto parent = parentWidget();
    if (!parent) {
        setParent(main_view_);
    }

    // 获取截图时只显示坐标系和真实机器人
    auto options = show_options_;
    setShowOptions(Ground | RealRobot);

    // 设置真实机器人关节角度和末端位姿, 获取截图
    setRealJointPositions(joints);
    setTcpPose(pose);

    QPixmap *t = new QPixmap(grab(QRect(0, 0, width(), height())));

    // 还原状态
    setShowOptions(options);
    if (!parent) {
        setParent(nullptr);
    }

    // 新位姿截图加入缓存
    cache_pixmap_.insert(key_val, t);

    last_roty = roty;

    return t;
}

void RenderForm::initView()
{
    camera_->setCamera({ 0, CAMERA_INIT_Y, CAMERA_INIT_Z },
                       { 0, CAMERA_INIT_Y, 0 }, { 0, 1, 0 });
    camera_->translate(0, default_down_move_, default_zoom_out_);
}

void RenderForm::fixDisplay()
{
    camera_->setRotateEnable({ false, false, false });
}

void RenderForm::freeDisplay()
{
    camera_->setRotateEnable({ false, true, false });
}

void RenderForm::setMoveTrack(void *wp,
                              const std::vector<std::vector<double>> &trackList)
{
    QMutexLocker lk(&mtx_waypoints_);
    if (waypoints_.contains(wp)) {
        auto &waypoint = waypoints_[wp];
        if (waypoint.move_traj == trackList) {
            return;
        }

        waypoint.move_traj = trackList;
        waypoint.updateTrajs();
    }
}

void RenderForm::setBlendTrack(
    void *wp, const std::vector<std::vector<double>> &trackList)
{
    QMutexLocker lk(&mtx_waypoints_);
    if (waypoints_.contains(wp)) {
        auto &waypoint = waypoints_[wp];
        if (waypoint.blend_traj == trackList) {
            return;
        }

        waypoint.blend_traj = trackList;
        waypoint.updateTrajs();
    }
}

bool RenderForm::hasWaypoint(void *wp)
{
    QMutexLocker lk(&mtx_waypoints_);
    return waypoints_.contains(wp);
}

bool RenderForm::addWaypoint(void *wp, const std::vector<double> &pose)
{
    if (pose.size() != 6) {
        return false;
    }

    QMutexLocker lk(&mtx_waypoints_);
    if (waypoints_.contains(wp)) {
        waypoints_[wp].setPose(pose);
        return false;
    } else {
        WaypointTrack wpt;
        render_manager_->addRenderer(wpt.r_waypoints_);
        render_manager_->addRenderer(wpt.r_traj_);
        wpt.setPose(pose);
        wpt.r_waypoints_->setVisible(
            ui->pbn_point->isHidden() ? false : ui->pbn_point->isChecked());
        wpt.r_traj_->setVisible(
            ui->pbn_track->isHidden() ? false : ui->pbn_track->isChecked());
        waypoints_[wp] = wpt;
        return true;
    }
}

void RenderForm::deleteWaypoint(void *wp)
{
    QMutexLocker lk(&mtx_waypoints_);
    if (waypoints_.contains(wp)) {
        render_manager_->removeRenderer(waypoints_[wp].r_waypoints_);
        render_manager_->removeRenderer(waypoints_[wp].r_traj_);
        waypoints_.remove(wp);
    }
}

void RenderForm::clearWaypoints()
{
    QMutexLocker lk(&mtx_waypoints_);

    for (auto &waypoint : waypoints_) {
        render_manager_->removeRenderer(waypoint.r_waypoints_);
        render_manager_->removeRenderer(waypoint.r_traj_);
    }

    waypoints_.clear();
}

void RenderForm::setCoordinatesVisiable(bool visiable)
{
    coordinates_visiabled_ = visiable;

    for (auto &r_coordinate : r_coordinates_) {
        r_coordinate->setVisible(coordinates_visiabled_ ||
                                 !ui->pbn_coord->isHidden() &&
                                     ui->pbn_coord->isChecked());
    }

    for (auto &r_coordinate_axis : r_coordinates_axis_) {
        r_coordinate_axis->setVisible(coordinates_visiabled_ ||
                                      !ui->pbn_coord->isHidden() &&
                                          ui->pbn_coord->isChecked());
    }
}

void RenderForm::setCoordinate(const QString &name,
                               const std::vector<double> &pose)
{
    if (pose.size() != 6) {
        removeCoordinate(name);
        return;
    }

    if (hasCoordinate(name)) {
        coordinates_[name] = pose;

        auto world_pose = Math::robotToWorld(pose);
        r_coordinates_axis_[name]->setPose(world_pose);
        r_coordinates_[name]->setPose(world_pose);
    } else {
        coordinates_[name] = pose;

        auto world_pose = Math::robotToWorld(pose);
        r_coordinates_axis_[name].reset(new AxisRenderer());
        r_coordinates_axis_[name]->setPose(world_pose);
        r_coordinates_axis_[name]->setVisible(coordinates_visiabled_ ||
                                              !ui->pbn_coord->isHidden() &&
                                                  ui->pbn_coord->isChecked());
        r_coordinates_axis_[name]->setShape(AXIS_DEFAULT_CONE_HEIGHT,
                                            AXIS_DEFAULT_CONE_RADIUS);
        r_coordinates_axis_[name]->setFixedSize(true,
                                                CAMERA_INIT_DISTANCE_LENGTH);
        render_manager_->addRenderer(r_coordinates_axis_.value(name));

        r_coordinates_[name].reset(new BoxRenderer());
        r_coordinates_[name]->setPose(world_pose);
        r_coordinates_[name]->setShape(800, 800, 0);
        r_coordinates_[name]->setColor({ 0.6, 0.6, 0.6, 0.5 });
        r_coordinates_[name]->setVisible(coordinates_visiabled_ ||
                                         !ui->pbn_coord->isHidden() &&
                                             ui->pbn_coord->isChecked());
        render_manager_->addRenderer(r_coordinates_.value(name));
    }
}

std::vector<double> RenderForm::getCoordinate(
    const QString &name, const std::vector<double> &default_pose)
{
    return coordinates_.value(name, default_pose);
}

bool RenderForm::hasCoordinate(const QString &name)
{
    return coordinates_.contains(name);
}

void RenderForm::removeCoordinate(const QString &name)
{
    coordinates_.remove(name);
    render_manager_->removeRenderer(r_coordinates_axis_.value(name));
    render_manager_->removeRenderer(r_coordinates_.value(name));
    r_coordinates_axis_.remove(name);
    r_coordinates_.remove(name);
}

void RenderForm::addPointCloud(const std::vector<std::vector<double>> &points,
                               const QString &name, const QColor &color)
{
    if (point_clouds_.contains(name)) {
        auto &point_cloud = point_clouds_[name];
        point_cloud.setPoints(points, color);
    } else {
        PointCloud point_cloud;
        render_manager_->addRenderer(point_cloud.r_point_cloud);
        point_cloud.setPoints(points, color);
        point_clouds_[name] = point_cloud;
    }
}

void RenderForm::removePointCloud(const QString &name)
{
    if (point_clouds_.contains(name)) {
        auto &point_cloud = point_clouds_[name];
        render_manager_->removeRenderer(point_cloud.r_point_cloud);
        point_clouds_.remove(name);
    }
}

bool RenderForm::hasPointCloud(const QString &name)
{
    return point_clouds_.contains(name);
}

void RenderForm::clearPointCloud()
{
    for (auto &point_cloud : point_clouds_) {
        render_manager_->removeRenderer(point_cloud.r_point_cloud);
    }
    point_clouds_.clear();
}

void RenderForm::setSafetyPlanesVisiable(bool visiable)
{
    safety_planes_visiabled_ = visiable;

    for (auto &r_safety_plane : r_safety_planes_) {
        r_safety_plane->setVisible(safety_planes_visiabled_);
    }

    for (auto &r_safety_plane_axis : r_safety_planes_axis_) {
        r_safety_plane_axis->setVisible(safety_planes_visiabled_);
    }
}

void RenderForm::addSafetyPlane(
    const QString &name, const std::pair<std::vector<double>, int8_t> &pose)
{
    if (pose.first.size() != 6) {
        return;
    }

    // 修改
    if (safety_planes_.contains(name)) {
        safety_planes_[name] = pose;

        // 机器人坐标在世界坐标系中的位置
        auto world_pose = Math::robotToWorld(pose.first);

        // 平面颜色
        QVector4D plane_color;
        switch (pose.second) {
        case 0:
            plane_color = QVector4D(0.0, 0.0, 0.0, 0.2);
            break;
        case 1:
            plane_color = QVector4D(0.12, 0.785, 0.12, 0.35);
            break;
        case 2:
            plane_color = QVector4D(0, 0.016, 1, 0.2);
            break;
        default:
            plane_color = QVector4D(0.12, 0.785, 0.12, 0.35);
            break;
        }

        // 安全平面渲染
        r_safety_planes_[name]->setPose(world_pose);
        r_safety_planes_[name]->setColor(plane_color);

        // 安全平面方向渲染
        r_safety_planes_axis_[name]->setPose(world_pose);
        r_safety_planes_axis_[name]->setColor(
            { 0.0, 0.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0, 0.0 },
            { plane_color[0], plane_color[1], plane_color[2],
              1.0 }); // 只显示 Z 轴颜色, 与平面颜色一致, 不透明
    }
    // 新增
    else {
        safety_planes_[name] = pose;

        // 平面颜色
        QVector4D plane_color;
        switch (pose.second) {
        case 0:
            plane_color = QVector4D(0.0, 0.0, 0.0, 0.2);
            break;
        case 1:
            plane_color = QVector4D(0.12, 0.785, 0.12, 0.35);
            break;
        case 2:
            plane_color = QVector4D(0, 0.016, 1, 0.2);
            break;
        default:
            plane_color = QVector4D(0.12, 0.785, 0.12, 0.35);
            break;
        }

        auto world_pose = Math::robotToWorld(pose.first);

        // NOTE: 这里必须先渲染坐标系, 再渲染平面, 否则透明度混合渲染会异常
        // 安全平面方向渲染
        // FIXME: 这里 Z 轴如果和安全平面颜色一致, 带透明度的话,
        // 投影过去会变成纯透明色.
        r_safety_planes_axis_[name].reset(new AxisRenderer());
        r_safety_planes_axis_[name]->setPose(world_pose);
        r_safety_planes_axis_[name]->setVisible(safety_planes_visiabled_);
        r_safety_planes_axis_[name]->setShape(AXIS_DEFAULT_CONE_HEIGHT,
                                              AXIS_DEFAULT_CONE_RADIUS);
        r_safety_planes_axis_[name]->setColor(
            { 0.0, 0.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0, 0.0 },
            { plane_color[0], plane_color[1], plane_color[2],
              1.0 }); // 只显示 Z 轴颜色, 与平面颜色一致, 不透明
        r_safety_planes_axis_[name]->setFixedSize(true,
                                                  CAMERA_INIT_DISTANCE_LENGTH);
        render_manager_->addRenderer(r_safety_planes_axis_.value(name));

        // 安全平面渲染
        r_safety_planes_[name].reset(new BoxRenderer());
        r_safety_planes_[name]->setPose(world_pose);
        r_safety_planes_[name]->setShape(800, 800, 0);
        r_safety_planes_[name]->setColor(plane_color);
        r_safety_planes_[name]->setVisible(safety_planes_visiabled_);
        render_manager_->addRenderer(r_safety_planes_.value(name));
    }
}

void RenderForm::removeSafetyPlane(const QString &name)
{
    if (!safety_planes_.contains(name)) {
        return;
    }

    safety_planes_.remove(name);

    render_manager_->removeRenderer(r_safety_planes_[name]);
    r_safety_planes_.remove(name);

    render_manager_->removeRenderer(r_safety_planes_axis_[name]);
    r_safety_planes_axis_.remove(name);
}

void RenderForm::removeAllSafetyPlanes()
{
    for (auto &r_safety_plane : r_safety_planes_) {
        render_manager_->removeRenderer(r_safety_plane);
    }

    for (auto &r_safety_plane_axis : r_safety_planes_axis_) {
        render_manager_->removeRenderer(r_safety_plane_axis);
    }

    safety_planes_.clear();
}

void RenderForm::renameSafetyPlane(const QString &old_name,
                                   const QString &new_name)
{
    auto value = safety_planes_.take(old_name);
    safety_planes_[new_name] = value;

    auto r_value = r_safety_planes_.take(old_name);
    safety_planes_[new_name] = value;

    auto r_value_axis = r_safety_planes_axis_.take(old_name);
    safety_planes_[new_name] = value;
}

void RenderForm::setObstacleMarkersVisiable(bool visiable)
{
    obstacle_markers_visiabled_ = visiable;

    for (auto &r_obstacle_marker_ : r_obstacle_markers_) {
        r_obstacle_marker_->setVisible(obstacle_markers_visiabled_);
    }
}

void RenderForm::addObstacleMarker(const QString &name,
                                   const std::vector<double> &pose,
                                   const std::vector<double> &size,
                                   const QVector4D &color)
{
    if (pose.size() != 6 || size.size() != 3) {
        return;
    }

    // 修改
    if (r_obstacle_markers_.contains(name)) {
        auto world_pose = Math::robotToWorld(pose);

        r_obstacle_markers_[name]->setPose(world_pose);
        r_obstacle_markers_[name]->setShape(size[0] * 1000, size[1] * 1000,
                                            size[2] * 1000);
        r_obstacle_markers_[name]->setColor(color);
    }
    // 新增
    else {
        auto world_pose = Math::robotToWorld(pose);

        r_obstacle_markers_[name].reset(new BoxRenderer());
        r_obstacle_markers_[name]->setPose(world_pose);
        r_obstacle_markers_[name]->setShape(size[0] * 1000, size[1] * 1000,
                                            size[2] * 1000);
        r_obstacle_markers_[name]->setColor(color);
        r_obstacle_markers_[name]->setVisible(obstacle_markers_visiabled_);
        render_manager_->addRenderer(r_obstacle_markers_.value(name));
    }
}

void RenderForm::removeObstacleMarker(const QString &name)
{
    if (!r_obstacle_markers_.contains(name)) {
        return;
    }

    render_manager_->removeRenderer(r_obstacle_markers_[name]);
    r_obstacle_markers_.remove(name);
}

void RenderForm::removeAllObstacleMarkers()
{
    for (auto &r_obstacle_marker : r_obstacle_markers_) {
        render_manager_->removeRenderer(r_obstacle_marker);
    }

    r_obstacle_markers_.clear();
}

void RenderForm::renameObstacleMarker(const QString &old_name,
                                      const QString &new_name)
{
    auto r_value = r_obstacle_markers_.take(old_name);
    r_obstacle_markers_[new_name] = r_value;
}

void RenderForm::setShowOptions(uint8_t options)
{
    show_options_ = options;
    ui->frm_graphics->setVisible(true);

    ui->pbn_ground->setVisible(options & Ground);
    ui->pbn_real_robot->setVisible(options & RealRobot);
    ui->pbn_target_robot->setVisible(options & TargetRobot);
    ui->pbn_coord->setVisible(options & Coordinate);
    ui->pbn_track->setVisible(options & Track);
    ui->pbn_point->setVisible(options & Point);

    // 基座显隐
    r_base_->setVisible(!ui->pbn_ground->isHidden() &&
                        ui->pbn_ground->isChecked());

    // 当前机器人显隐
    auto visiable =
        !ui->pbn_real_robot->isHidden() && ui->pbn_real_robot->isChecked();
    r_real_robot_->setVisible(visiable);
    r_real_robot_pose_->setVisible(visiable);

    // 目标机器人显隐
    updateTargetVisiable();

    // 用户坐标系 坐标轴显隐
    visiable = coordinates_visiabled_ ||
               !ui->pbn_coord->isHidden() && ui->pbn_coord->isChecked();
    for (auto &c : r_coordinates_axis_) {
        c->setVisible(visiable);
    }
    for (auto &c : r_coordinates_) {
        c->setVisible(visiable);
    }

    // 路点显隐
    visiable = !ui->pbn_point->isHidden() && ui->pbn_point->isChecked();
    for (auto &waypoint : waypoints_) {
        waypoint.r_waypoints_->setVisible(visiable);
    }

    // 轨迹显隐
    visiable = !ui->pbn_track->isHidden() && ui->pbn_track->isChecked();
    for (auto &waypoint : waypoints_) {
        waypoint.r_traj_->setVisible(visiable);
    }

    // 视野切换
    ui->pbn_zoom_out->setVisible(options & View);
    ui->pbn_up_move->setVisible(options & View);
    ui->pbn_zoom_in->setVisible(options & View);
    ui->pbn_left_move->setVisible(options & View);
    ui->pbn_front_view->setVisible(options & View);
    ui->pbn_right_move->setVisible(options & View);
    ui->pbn_left_rotate->setVisible(options & View);
    ui->pbn_down_move->setVisible(options & View);
    ui->pbn_right_rotate->setVisible(options & View);
}

RenderForm::WaypointTrack::WaypointTrack()
{
    r_waypoints_.reset(new SphereRenderer());
    r_waypoints_->setShape(10.0, 10, 10, { 0.0, 1.0, 0.0, 1.0 });

    r_traj_.reset(new LineRenderer());
    r_traj_->setPosition(QVector3D(0.0, 0.0, 0.0));
}

void RenderForm::WaypointTrack::setPose(const std::vector<double> &pose)
{
    if (this->pose == pose) {
        return;
    }

    this->pose = pose;

    auto world_pose = Math::robotToWorld(pose);
    r_waypoints_->setPosition(
        QVector3D(world_pose[0], world_pose[1], world_pose[2]));
}

void RenderForm::WaypointTrack::updateTrajs()
{
    // 构建轨迹顶点数据
    QVector<QVector3D> points;
    int move_traj_size = move_traj.size();
    int blend_traj_size = blend_traj.size();
    points.resize(move_traj_size + blend_traj_size);

    // 路径
    for (int i = 0; i < move_traj_size; i++) {
        auto world_pose = Math::robotToWorld(move_traj[i]);
        points[i] = QVector3D(world_pose[0], world_pose[1], world_pose[2]);
    }

    // 交融
    for (int i = 0; i < blend_traj_size; i++) {
        auto world_pose = Math::robotToWorld(blend_traj[i]);
        points[move_traj_size + i] =
            QVector3D(world_pose[0], world_pose[1], world_pose[2]);
    }

    // 设置轨迹数据
    r_traj_->setShape(points, QVector4D(0.5647, 0.9333, 0.5647, 1.0));
}

RenderForm::PointCloud::PointCloud()
{
    r_point_cloud.reset(new PointCloudRenderer());
}

void RenderForm::PointCloud::setPoints(
    const std::vector<std::vector<double>> points, const QColor &color)
{
    if (this->points == points && this->color == color) {
        return;
    }
    this->points = points;
    this->color = color;

    // 提取 x, y, z
    QVector<QVector3D> _points;
    _points.resize(points.size());
    for (int i = 0; i < points.size(); i++) {
        auto world_pose = Math::robotToWorld(points[i]);
        _points[i] = QVector3D(world_pose[0], world_pose[1], world_pose[2]);
    }

    // 设置渲染数据
    r_point_cloud->setPoints(
        _points, QVector3D(color.redF(), color.greenF(), color.blueF()), 3.0);
}
